#include "../include/Hero.h"
#include "../include/Global.h"

#include "SDL/SDL.h"
#include "SDL/SDL_opengl.h"

double _PI = 3.14;

Hero::Hero(Position P) : Minion(P)
{
    bullet.SetBulletDamage(20);
    building = false;
    slowBullet.SetRange(5);
    UsedSkill = false;
    Target = Position(100,100);
      slowBullet.setInitialPosition(Pos);


}

std::vector<Position> Hero::bestPath(const std::vector<Position>& towerPos, const Position destination)
{

        // Pos = PrevPos; // Need to fix this it makes it teleport.
    std::vector<Position> solution;
    int totalnodes  = 0;
    float DiagonalDistance = sqrtf(2);
    goal = destination;
    bool found = false;
    std::vector<Position> towers = towerPos;
    std::vector<Position> visited = towerPos;

    std::priority_queue<Position, std::vector < std::vector < Position > >, CompareDist> pathsQ;
    std::vector<Position> myPos;
    if(MyBestPath.size() <=2)
    {
        myPos.push_back(PrevPos); //starting point
    }
    if(MyBestPath.size() >2)
    {
        myPos.push_back(MyBestPath[0]);
        myPos.push_back(MyBestPath[1]);
    }
    if(hasBackPath(towerPos, destination, myPos[myPos.size() - 1]))
    {
    myPos.push_back( Position(0,0) ); //This hold the total distance.
    pathsQ.push(myPos);

//Now we can start the PathFinding.
    while(!pathsQ.empty()&& !found)
    {
        totalnodes++;
    //    std::priority_queue<Position, std::vector < std::vector < Position > >, CompareStartDist> temp = pathsQ;
        std::vector<Position> tempPath = pathsQ.top();
        pathsQ.pop();
        Position tempPos = tempPath[tempPath.size()-2]; // The last one is the Dist need to go one back
        visited.push_back(tempPos);
//        while(!temp.empty()){
//        std::cout << temp.top().size() << std::endl;
//        temp.pop();
//
//
//        }
//       std::cout << tempPos.Xpos <<" , " << tempPos.Ypos << std::endl;
//       std::cout << goal.Xpos <<" , " << goal.Ypos << std::endl;


    //SetPenalty(tempPos, goal, Lpen, Rpen,  Dpen, Upen);



        if(tempPos == goal)  //found it;
        {
            found = true;
            tempPath[tempPath.size() - 1] = tempPos;
            solution = tempPath;
        }
        else{ // I have not found the answer
        tempPos.Xpos -= 1;
        if(!HaveVisited(visited,tempPos)) //Check left
        {
        //std::cout << " Checking Left" << std::endl;
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
//            std::cout << Distance.Xpos << std::endl;
            std::vector<Position> leftPath = tempPath;
            leftPath[leftPath.size() -1] = tempPos; // Sets the final spot to TemPpos

            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - goal.Xpos), 2) + powf(fabs(tempPos.Ypos - goal.Ypos), 2) ) + Distance.Ypos; // Estimated Distance + actual
            Distance.Ypos += 1 + (1/100); // Total Grid Distance
//            std::cout << Distance.Xpos << std::endl;
            visited.push_back(tempPos);
            leftPath.push_back(Distance);
            pathsQ.push(leftPath);
        }
        tempPos.Xpos += 2;
        if(!HaveVisited(visited,tempPos)) //Check right
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> rightPath = tempPath;
            rightPath[rightPath.size() -1] = tempPos; // Sets the final spot to TemPpos

            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - goal.Xpos), 2) + powf(fabs(tempPos.Ypos - goal.Ypos), 2) ) + Distance.Ypos;  // Estimated Distance + actual
             Distance.Ypos += 1 + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            rightPath.push_back(Distance);
            pathsQ.push(rightPath);
        }
        tempPos.Xpos -= 1; //center back up
        tempPos.Ypos += 1;
        if(!HaveVisited(visited,tempPos)) //Check up
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> upPath = tempPath;
            upPath[upPath.size() -1] = tempPos; // Sets the final spot to TemPpos

            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - goal.Xpos), 2) + powf(fabs(tempPos.Ypos - goal.Ypos), 2) ) + Distance.Ypos;  // Estimated Distance + actual
            Distance.Ypos += 1 + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            upPath.push_back(Distance);
            pathsQ.push(upPath);
        }
        tempPos.Ypos -= 2;
        if(!HaveVisited(visited,tempPos)) //Check down
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> downPath = tempPath;
            downPath[downPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - goal.Xpos), 2) + powf( fabs(tempPos.Ypos - goal.Ypos), 2) ) + Distance.Ypos;  // Estimated Distance
             Distance.Ypos += 1 + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            downPath.push_back(Distance);
            pathsQ.push(downPath);
        }
        tempPos.Ypos += 1; // Puts us back at center
        tempPos.Ypos += 1; tempPos.Xpos -= 1; // Puts us at the top left position.
         if(!HaveVisited(visited,tempPos) && // Check Spot with have I been there
             (!HaveVisited(towers,Position(tempPos.Xpos, tempPos.Ypos - 1)) && // Check Left with towers
             !HaveVisited(towers,Position(tempPos.Xpos + 1, tempPos.Ypos) ) ) ) //Check Up with towers
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> UpLeftPath = tempPath;
            UpLeftPath[UpLeftPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - goal.Xpos), 2) + powf( fabs(tempPos.Ypos - goal.Ypos), 2) ) + Distance.Ypos;  // Estimated Distance
             Distance.Ypos += DiagonalDistance + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            UpLeftPath.push_back(Distance);
            pathsQ.push(UpLeftPath);
        }
        tempPos.Xpos += 2; //Moves us to the Top Right Node
        if(!HaveVisited(visited,tempPos) && // Check Spot with have I been there
             (!HaveVisited(towers,Position(tempPos.Xpos, tempPos.Ypos - 1)) && // Check Right with towers
             !HaveVisited(towers,Position(tempPos.Xpos - 1, tempPos.Ypos) )) ) //Check Up with towers
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> UpRightPath = tempPath;
            UpRightPath[UpRightPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - goal.Xpos), 2) + powf( fabs(tempPos.Ypos - goal.Ypos), 2) ) + Distance.Ypos;  // Estimated Distance
             Distance.Ypos += DiagonalDistance + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            UpRightPath.push_back(Distance);
            pathsQ.push(UpRightPath);
        }

        tempPos.Ypos -= 2; //Moves us to the Bottom Right Node
        if(!HaveVisited(visited,tempPos) && // Check Spot with have I been there
            ( !HaveVisited(towers,Position(tempPos.Xpos, tempPos.Ypos + 1)) && // Check Right with towers
             !HaveVisited(towers,Position(tempPos.Xpos - 1, tempPos.Ypos) ) ) ) //Check Down with towers
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> DownRightPath = tempPath;
            DownRightPath[DownRightPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - goal.Xpos), 2) + powf( fabs(tempPos.Ypos - goal.Ypos), 2) ) + Distance.Ypos;  // Estimated Distance
             Distance.Ypos += DiagonalDistance + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            DownRightPath.push_back(Distance);
            pathsQ.push(DownRightPath);
        }

        tempPos.Xpos -= 2; //Moves us to the Bottom Left Node
        if(!HaveVisited(visited,tempPos) && // Check Spot with have I been there
             (!HaveVisited(towers,Position(tempPos.Xpos, tempPos.Ypos + 1)) &&// Check Left with towers
             !HaveVisited(towers,Position(tempPos.Xpos + 1, tempPos.Ypos) ) ) ) //Check Down with towers
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> DownLeftPath = tempPath;
            DownLeftPath[DownLeftPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - goal.Xpos), 2) + powf( fabs(tempPos.Ypos - goal.Ypos), 2) ) + Distance.Ypos;  // Estimated Distance
             Distance.Ypos += DiagonalDistance + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            DownLeftPath.push_back(Distance);
            pathsQ.push(DownLeftPath);
        }


        }
        //I need to check to see if Goal is less than position at the start. Or maybe I should be updating
        // It real time.

    }
    std::cout << totalnodes << std::endl;
    MyBestPath = solution;
    return solution;
    }
    return solution;
}
bool Hero::hasBackPath(const std::vector<Position>& towerPos, const Position start, const Position final)
{
        // Pos = PrevPos; // Need to fix this it makes it teleport.
    std::vector<Position> solution;
    int totalnodes  = 0;
    float DiagonalDistance = sqrtf(2);
    bool found = false;
    std::vector<Position> towers = towerPos;
    std::vector<Position> visited;
    if( towers[towers.size()-1] == final){
    for(int k = 0; k < towers.size() -1; k++)
    visited.push_back(towers[k]);
    }
    else { visited = towers; }
    std::priority_queue<Position, std::vector < std::vector < Position > >, CompareDist> pathsQ;
    std::vector<Position> myPos;
    myPos.push_back( start );
    myPos.push_back( Position(0,0) ); //This hold the total distance.
    pathsQ.push(myPos);

//Now we can start the PathFinding.
    while(!pathsQ.empty()&& !found)
    {
        totalnodes++;
    //    std::priority_queue<Position, std::vector < std::vector < Position > >, CompareStartDist> temp = pathsQ;
        std::vector<Position> tempPath = pathsQ.top();
        pathsQ.pop();
        Position tempPos = tempPath[tempPath.size()-2]; // The last one is the Dist need to go one back
        visited.push_back(tempPos);
//        while(!temp.empty()){
//        std::cout << temp.top().size() << std::endl;
//        temp.pop();
//
//
//        }
//       std::cout << tempPos.Xpos <<" , " << tempPos.Ypos << std::endl;
//       std::cout << goal.Xpos <<" , " << goal.Ypos << std::endl;


    //SetPenalty(tempPos, goal, Lpen, Rpen,  Dpen, Upen);



        if(tempPos == final)  //found it;
        {
            found = true;
            tempPath[tempPath.size() - 1] = tempPos;
            solution = tempPath;
        }
        else{ // I have not found the answer
        tempPos.Xpos -= 1;
        if(!HaveVisited(visited,tempPos)) //Check left
        {
        //std::cout << " Checking Left" << std::endl;
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
//            std::cout << Distance.Xpos << std::endl;
            std::vector<Position> leftPath = tempPath;
            leftPath[leftPath.size() -1] = tempPos; // Sets the final spot to TemPpos

            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - final.Xpos), 2) + powf(fabs(tempPos.Ypos - final.Ypos), 2) ); // Estimated Distance + actual
            Distance.Ypos += 1 + (1/100); // Total Grid Distance
//            std::cout << Distance.Xpos << std::endl;
            visited.push_back(tempPos);
            leftPath.push_back(Distance);
            pathsQ.push(leftPath);
        }
        tempPos.Xpos += 2;
        if(!HaveVisited(visited,tempPos)) //Check right
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> rightPath = tempPath;
            rightPath[rightPath.size() -1] = tempPos; // Sets the final spot to TemPpos

            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - final.Xpos), 2) + powf(fabs(tempPos.Ypos - final.Ypos), 2) );  // Estimated Distance + actual
             Distance.Ypos += 1 + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            rightPath.push_back(Distance);
            pathsQ.push(rightPath);
        }
        tempPos.Xpos -= 1; //center back up
        tempPos.Ypos += 1;
        if(!HaveVisited(visited,tempPos)) //Check up
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> upPath = tempPath;
            upPath[upPath.size() -1] = tempPos; // Sets the final spot to TemPpos

            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - final.Xpos), 2) + powf(fabs(tempPos.Ypos - final.Ypos), 2) );  // Estimated Distance + actual
            Distance.Ypos += 1 + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            upPath.push_back(Distance);
            pathsQ.push(upPath);
        }
        tempPos.Ypos -= 2;
        if(!HaveVisited(visited,tempPos)) //Check down
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> downPath = tempPath;
            downPath[downPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - final.Xpos), 2) + powf( fabs(tempPos.Ypos - final.Ypos), 2) );  // Estimated Distance
             Distance.Ypos += 1 + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            downPath.push_back(Distance);
            pathsQ.push(downPath);
        }
        tempPos.Ypos += 1; // Puts us back at center
        tempPos.Ypos += 1; tempPos.Xpos -= 1; // Puts us at the top left position.
         if(!HaveVisited(visited,tempPos) && // Check Spot with have I been there
             (!HaveVisited(towers,Position(tempPos.Xpos, tempPos.Ypos - 1)) && // Check Left with towers
             !HaveVisited(towers,Position(tempPos.Xpos + 1, tempPos.Ypos) ) ) ) //Check Up with towers
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> UpLeftPath = tempPath;
            UpLeftPath[UpLeftPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - final.Xpos), 2) + powf( fabs(tempPos.Ypos - final.Ypos), 2) );  // Estimated Distance
             Distance.Ypos += DiagonalDistance + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            UpLeftPath.push_back(Distance);
            pathsQ.push(UpLeftPath);
        }
        tempPos.Xpos += 2; //Moves us to the Top Right Node
        if(!HaveVisited(visited,tempPos) && // Check Spot with have I been there
             (!HaveVisited(towers,Position(tempPos.Xpos, tempPos.Ypos - 1)) && // Check Right with towers
             !HaveVisited(towers,Position(tempPos.Xpos - 1, tempPos.Ypos) )) ) //Check Up with towers
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> UpRightPath = tempPath;
            UpRightPath[UpRightPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - final.Xpos), 2) + powf( fabs(tempPos.Ypos - final.Ypos), 2) );  // Estimated Distance
             Distance.Ypos += DiagonalDistance + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            UpRightPath.push_back(Distance);
            pathsQ.push(UpRightPath);
        }

        tempPos.Ypos -= 2; //Moves us to the Bottom Right Node
        if(!HaveVisited(visited,tempPos) && // Check Spot with have I been there
            ( !HaveVisited(towers,Position(tempPos.Xpos, tempPos.Ypos + 1)) && // Check Right with towers
             !HaveVisited(towers,Position(tempPos.Xpos - 1, tempPos.Ypos) ) ) ) //Check Down with towers
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> DownRightPath = tempPath;
            DownRightPath[DownRightPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - final.Xpos), 2) + powf( fabs(tempPos.Ypos - final.Ypos), 2) );  // Estimated Distance
             Distance.Ypos += DiagonalDistance + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            DownRightPath.push_back(Distance);
            pathsQ.push(DownRightPath);
        }

        tempPos.Xpos -= 2; //Moves us to the Bottom Left Node
        if(!HaveVisited(visited,tempPos) && // Check Spot with have I been there
             (!HaveVisited(towers,Position(tempPos.Xpos, tempPos.Ypos + 1)) &&// Check Left with towers
             !HaveVisited(towers,Position(tempPos.Xpos + 1, tempPos.Ypos) ) ) ) //Check Down with towers
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> DownLeftPath = tempPath;
            DownLeftPath[DownLeftPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos = sqrtf( powf(fabs(tempPos.Xpos - final.Xpos), 2) + powf( fabs(tempPos.Ypos - final.Ypos), 2) );  // Estimated Distance
             Distance.Ypos += DiagonalDistance + 1/100; // Total Grid Distance
            visited.push_back(tempPos);
            DownLeftPath.push_back(Distance);
            pathsQ.push(DownLeftPath);
        }


        }
        //I need to check to see if Goal is less than position at the start. Or maybe I should be updating
        // It real time.

    }
    std::cout << totalnodes << std::endl;
    return found;
}

//Autofires at a minion if they are in range
bool Hero :: AutoFireAt(std::vector<Minion> &min)
{

    bullet.setInitialPosition(Pos);
    if(bullet.getPos().Xpos != 0 || bullet.getPos().Ypos != 0)
    {
        if(bullet.Fire(min, bullet.minionIndex))
            bullet.hitEffect(min, bullet.minionIndex);
    }
    if(bullet.TestFire(min))
    {
        if(bullet.Fire(min, bullet.minionIndex))
            bullet.hitEffect(min, bullet.minionIndex);
    }
    bullet.draw();

}


//Sets the UsedSkill boolean to true, and will return interegers to represent the type of skill used.
void Hero :: UseSkill()
{
    UsedSkill = true;
}

//Handles the actual process of the skills, testing, shooting, and damage
bool Hero :: Skill(std::vector<Minion> &min)
{

    if(UsedSkill)
    {

        slowBullet.setInitialPosition(Pos);
        if(slowBullet.TestFire(Target))
        {
            if(slowBullet.Fire(Target))
            {

                slowBullet.setPos(Position(0,0));
                slowBullet.Zpos = 2.4;
                slowBullet.Timer.start();
                slowBullet.hitEffect(min,Target);
                 Target = Position(100,100);
                  UsedSkill = false;
                return true;

            }
            slowBullet.draw();
        }
        return false;




    }

    return false;
}

//Checks to see if the delay is up on the slowSkill, returns true if it is
bool Hero :: CheckSlowSkillTimer()
{
    if(slowBullet.CheckTimerDelay())
        return true;
    else
        return false;
}




//Draws the range for the type of bullet around the hero
void Hero :: SkillTarget(int type)
{

    switch(type)
    {
    case SlowSkill:
    {
        glPushMatrix();
        glTranslatef(0,0,0.2);
        glColor4f(0,0.3,0.4,0.5);
        glBegin(GL_TRIANGLE_FAN);
        glVertex2f(Pos.Xpos, Pos.Ypos);
        for (int angle = 0; angle <= 360; angle+= 3)
            glVertex2f(Pos.Xpos + sin(angle*_PI/180) * slowBullet.GetRange(), Pos.Ypos + cos(angle*_PI/180) * slowBullet.GetRange());
        glEnd();
        glPopMatrix();
        break;
    }
    }
}
//Handles the drawing of the timer
void Hero :: drawTimer()
{

    if(building)
    {

        glPushMatrix();
        glTranslatef(Pos.Xpos,Pos.Ypos, GROUND + 3);
        glBegin(GL_QUADS);
        glColor3f(0.4f, 0.8f, 0.8f);
        glVertex3f(-0.35f, -0.05f, 1.5f);
        glVertex3f(0.35f, -0.05f, 1.5f);
        glVertex3f(0.35f, 0.05f, 1.5f);
        glVertex3f(-0.35f, 0.05f, 1.5f);
        glEnd();


        glPushMatrix();
        glColor3f(1, 1, 0.0f);
        glTranslatef(-0.35,0,GROUND + 0.0001);
        glBegin(GL_QUADS);
        glVertex3f(0, -0.05f, 1.5f);
        glVertex3f(0.7f *(BuildTime.elapsedTime()/TotalBuildTime), -0.05f, 1.5f); // Stopped here
        glVertex3f(0.7f * (BuildTime.elapsedTime()/TotalBuildTime), 0.05f, 1.5f);
        glVertex3f(0, 0.05f, 1.5f);
        glEnd();
        glPopMatrix();
        glPopMatrix();
    }
}


//Events which happen when Hero starts building
void Hero :: beginBuild()
{
    BuildTime.start();
    building = true;
}

//Checks the timer of building, stores the building time of each turret
bool Hero :: checkBuildTimer(double type)
{

    TotalBuildTime = 1.0;
    if(BuildTime.elapsedTime() > TotalBuildTime)
    {
        building = false;
        return true;
    }
    else
    {
        return false;
        building = false;
    }
}

void Hero :: SetTarget(Position P)
{
    Target = P;
}
void Hero::Update()
{

    if (MyBestPath.size() >= 2)
    {
        if(AtNextPoint(PrevPos, Pos, MyBestPath[1])) // I am at the next Pos
        {
            PrevPos = MyBestPath[1];
            Pos = MyBestPath[1];
            MyBestPath.erase(MyBestPath.begin());
        }
        float Xmovement = MyBestPath[1].Xpos - PrevPos.Xpos;
        float Ymovement = MyBestPath[1].Ypos - PrevPos.Ypos;
        Pos.Xpos += (double)(Xmovement*GetSpeed());
        Pos.Ypos += (double)(Ymovement*GetSpeed());
    }
    if(slow)
        CheckSlowDuration();
}



Hero::~Hero()
{

}
